#-*- coding:utf-8 -*-
import sim

print ('Program started')
sim.simxFinish(-1) # just in case, close all opened connections
clientID=sim.simxStart('127.0.0.1',19999,True,True,-500000,5) # Connect to V-REP, set a very large time-out for blocking commands
if clientID!=-1:
    print ('Connected to remote API server')

    emptyBuff = bytearray()

    # Start the simulation:
    sim.simxStartSimulation(clientID,sim.simx_opmode_oneshot_wait)

    # Load a robot instance:    res,retInts,retFloats,retStrings,retBuffer=vrep.simxCallScriptFunction(clientID,'remoteApiCommandServer',vrep.sim_scripttype_childscript,'loadRobot',[],[0,0,0,0],['d:/v_rep/qrelease/release/test.ttm'],emptyBuff,vrep.simx_opmode_oneshot_wait)
    #    robotHandle=retInts[0]
    
    # Retrieve some handles:
    res,robotHandle=sim.simxGetObjectHandle(clientID,'UR5#',sim.simx_opmode_oneshot_wait)
    print(robotHandle)  
    res,target1=sim.simxGetObjectHandle(clientID,'testPose1#',sim.simx_opmode_oneshot_wait)
    print(target1)

    # Retrieve the poses (i.e. transformation matrices, 12 values, last row is implicit) of some dummies in the scene
    res,retInts,target1Pose,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'getObjectPose',[target1],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
    print("target1Pose:")
    print(target1Pose)
    
    res,retInts,robotInitialState,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'getRobotState',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
    print("robotInitialState")
    print(robotInitialState)

     # Some parameters:
    approachVector=[0,0,0] # often a linear approach is required. This should also be part of the calculations when selecting an appropriate state for a given pose
    maxConfigsForDesiredPose=100 # we will try to find 10 different states corresponding to the goal pose and order them according to distance from initial state
    maxTrialsForConfigSearch=300 # a parameter needed for finding appropriate goal states
    searchCount=4 # how many times OMPL will run for a given task
    minConfigsForPathPlanningPath=400 # interpolation states for the OMPL path
    minConfigsForIkPath=100 # interpolation states for the linear approach path
    collisionChecking=1 # whether collision checking is on or off

    inInts=[robotHandle,collisionChecking,minConfigsForPathPlanningPath,searchCount]

    #等待直到末端运动并且判断目标点是否发生移动，如果发生移动，检测移动
    #--------------------------------------------------------------
    # 非常重要，搞明白
    #--------------------------------------------------------------
    isnotrunningpath = False
    while not isnotrunningpath:

        # res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'isnotRunningThroughPath',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
        # isnotrunningpath_not = retInts[0]==1    #false

        res,retInts,robotCurrentConfig,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'getRobotState',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
        print("robotCurrentConfig:")
        #  2.86102294921875e-06, -1.5707789659500122, 0.0, 3.4809112548828125e-05, -2.0265579223632812e-05, -2.384185791015625e-07
        print(robotCurrentConfig)

        #获取目标位置的关节位置
        res,retInts,robotInitialState_xin,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'UR5_scripe_xin',sim.sim_scripttype_childscript,'guanjiejiaodu',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
        print("robotInitialState_xin:")
        print(robotInitialState_xin)

        inFloats = robotCurrentConfig+robotInitialState_xin     #jiqiren chushi jiaodu jia txt guanjie jiaodu
        #  print("inFloats:")
        #  2.86102294921875e-06, -1.5707789659500122, 0.0, 3.4809112548828125e-05, -2.0265579223632812e-05, -2.384185791015625e-07, -1.01, -1.13, 0.79, 0.34, 2.14, 0.72
        #  print(inFloats)

        res,retInts,path,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'findPath_goalIsState',inInts,inFloats,[],emptyBuff,sim.simx_opmode_oneshot_wait)
        if (res==0) and len(path)>0:

            # Visualize the path:
            res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'visualizePath',[robotHandle,255,0,255],path,[],emptyBuff,sim.simx_opmode_oneshot_wait)
            lineHandle=retInts[0]
            
            # Make the robot follow the path:
            res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'runThroughPath',[robotHandle],path,[],emptyBuff,sim.simx_opmode_oneshot_wait)
            
            # Wait until the end of the movement:
            runningPath = True
            while runningPath:
                res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'isRunningThroughPath',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
                runningPath=retInts[0]==1
                
            # Clear the path visualization:
            res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'removeLine',[lineHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)

            #get reverse path
            # 机械臂末端返回原点
            #
            #res,retInts,path,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'goRetPath',[robotHandle],path,[],emptyBuff,sim.simx_opmode_oneshot_wait)  

            # Visualize the path:
            #res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'visualizePath',[robotHandle,255,0,255],path,[],emptyBuff,sim.simx_opmode_oneshot_wait)
            #lineHandle=retInts[0]

            #time.sleep(1)

            # Make the robot follow the path:
            #res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'runThroughPath',[robotHandle],path,[],emptyBuff,sim.simx_opmode_oneshot_wait)
            
            # Wait until the end of the movement:
            #runningPath=True
            #while runningPath:
                #res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'isRunningThroughPath',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
                #runningPath=retInts[0]==1
                
            # Clear the path visualization:
            #res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'removeLine',[lineHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
    # Stop simulation:
    sim.simxStopSimulation(clientID,sim.simx_opmode_oneshot_wait)
    # Now close the connection to V-REP:
    sim.simxFinish(clientID)
else:
    print ('Failed connecting to remote API server')
print ('Program ended')